#ifndef MY_INC_PHY_VSG_H
#define MY_INC_PHY_VSG_H

#include "MY_INC/PHY/PHY_PQ.h" // CORDIC库
#include <math.h>
#include <stdbool.h>
#include <stdint.h>

typedef struct
{

    float Ud;        // 有功功率
    float Uq;        // 无功功率
    float Id;        // 有功功率
    float Iq;        // 无功功率
    float Pe_Actual; // 有功功率
    float Qe_Actual; // 无功功率
    float Pe_Disp;   // 有功功率
    float Qe_Disp;   // 无功功率
    float coefficient;
    float PF; // PF值
} PQ_Typedef;
void               PQ_Process_Init(PQ_Typedef *PQ, float coefficient);
void               PQ_Process_Reset(PQ_Typedef *PQ);
static inline void PF_Process_Inline(PQ_Typedef *power);
static inline void PQ_Process_Inline(PQ_Typedef *power, float Ud, float Uq, float Id, float Iq);
static inline void PQ_Process_Inline(PQ_Typedef *power, float Ud, float Uq, float Id, float Iq)
{

    power->Ud = 0.01f * Ud + 0.99f * power->Ud;
    power->Uq = 0.01f * Uq + 0.99f * power->Uq;
    power->Id = 0.01f * Id + 0.99f * power->Id;
    power->Iq = 0.01f * Iq + 0.99f * power->Iq;

    power->Pe_Actual = (power->Ud * power->Id + power->Uq * power->Iq) * 0.5f * power->coefficient; // 低通滤波
    power->Qe_Actual = (power->Uq * power->Id - power->Ud * power->Iq) * 0.5f * power->coefficient; // 低通滤波

    power->Pe_Disp = power->Pe_Actual * 0.001f + 0.999f * power->Pe_Disp;
    power->Qe_Disp = power->Qe_Actual * 0.001f + 0.999f * power->Qe_Disp;
}
static inline void PF_Process_Inline(PQ_Typedef *power)
{

    if ((power->Pe_Disp > -0.5f)&& (power->Pe_Disp < 0.5f) && (power->Qe_Actual < 0.5f)&&(power->Qe_Actual > -0.5f))
    {
        power->PF = 0.0f;
        return;
    }
    power->PF = 0.01f * (power->Pe_Disp / (sqrtf(power->Pe_Disp * power->Pe_Disp + power->Qe_Disp * power->Qe_Disp))) +
                0.99f * power->PF;

    if (power->PF > 1.0f)
        power->PF = 1.0f;
    else if (power->PF < -1.0f)
        power->PF = -1.0f;
}

#endif // VSG_TYPES_H
